#include "getAmmo.h"
#include "CSerialPort.h"
#include "global.h"

extern CSerialPort myserialport;

typedef pcl::PointXYZ PointT;
using namespace openni;
using namespace std;


SimpleOpenNIViewer::SimpleOpenNIViewer() : viewer("PCL OpenNI Viewer") {}  // Construct a cloud viewer, with a window name

													   // ����ص�����cloud_cb_,��ȡ������ʱ�����ݽ��д���
void SimpleOpenNIViewer::cloud_cb_(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud)
{
	if (!viewer.wasStopped())
	{
			int times = 0;
			short pre_data[10][4];//0 x;1 z;2 size;  int *p
			
			pcl::PassThrough<PointT> pass;                         //直通滤波对象
			pcl::VoxelGrid<PointT> sor;

			pcl::NormalEstimationOMP<PointT, pcl::Normal> ne;        //法线估计对象
			pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne1;
			pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg; //分割对象
			pcl::ExtractIndices<PointT> extract;                   //点提取对象
			//pcl::search::KdTree<PointT>::Ptr tr ee(new pcl::search::KdTree<PointT>());
			pcl::EuclideanClusterExtraction<PointT> ec;

			pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
			pcl::PointCloud<PointT>::Ptr cloud_viewer(new pcl::PointCloud<PointT>);
			pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
			pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients);
			pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);

			//pcl::PointCloud<PointT>::Ptr cloud_plane(new pcl::PointCloud<PointT>());

			char send_data[8];
			short x,angle, z = 0.5;
			float y_min = 0.265;
								  // Build a passthrough filter to remove spurious NaNs

			pass.setInputCloud(cloud);
			pass.setFilterFieldName("y");
			pass.setFilterLimits(y_min_getammo,y_max_getammo);//-0.04, 0.06    -0.08, 0.01   table0.04, 0.14
			pass.filter(*cloud_viewer);

			sor.setInputCloud(cloud_viewer);
			sor.setLeafSize(0.003f, 0.003f, 0.003f);
			sor.filter(*cloud_filtered);


			//std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;
			pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);
			pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
			pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
			normal_estimator.setSearchMethod(tree);
			normal_estimator.setInputCloud(cloud_filtered);
			normal_estimator.setKSearch(5);
			normal_estimator.compute(*normals);


			pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
			std::vector<pcl::PointIndices> cluster_indices;
			reg.setMinClusterSize(900);
			reg.setMaxClusterSize(2500);
			reg.setSearchMethod(tree);
			reg.setNumberOfNeighbours(10);
			reg.setInputCloud(cloud_filtered);
			reg.setInputNormals(normals);
			reg.setSmoothnessThreshold(11 / 180.0 * M_PI);
			reg.setCurvatureThreshold(0.5);
			reg.extract(cluster_indices);

			pcl::PointCloud<PointT>::Ptr all_cloud_cluster(new pcl::PointCloud<PointT>);
			all_cloud_cluster->resize(cloud_filtered->size());

			for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
			{
				pcl::PointCloud<PointT>::Ptr cloud_cluster(new pcl::PointCloud<PointT>);

				for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
					cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
				cloud_cluster->width = cloud_cluster->points.size();
				cloud_cluster->height = 1;
				cloud_cluster->is_dense = true;
				cout << cloud_cluster->size() << endl;
				pcl::MomentOfInertiaEstimation <PointT> feature_extractor;
				feature_extractor.setInputCloud(cloud_cluster);
				feature_extractor.compute();

				pcl::PointXYZ min_point_AABB;
				pcl::PointXYZ max_point_AABB;
				Eigen::Vector3f major_vector, middle_vector, minor_vector;
				Eigen::Vector3f mass_center;

				feature_extractor.getAABB(min_point_AABB, max_point_AABB);
				feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);
				feature_extractor.getMassCenter(mass_center);


				//pcl::PointXYZ x_axis(major_vector(0) + mass_center(0), major_vector(1) + mass_center(1), major_vector(2) + mass_center(2));
				//pcl::PointXYZ y_axis(middle_vector(0) + mass_center(0), middle_vector(1) + mass_center(1), middle_vector(2) + mass_center(2));
				pcl::PointXYZ z_axis(minor_vector(0) + mass_center(0), minor_vector(1) + mass_center(1), minor_vector(2) + mass_center(2));

				float z_angle, real_angle,lenght;
				z_angle = atan2(z_axis.z, z_axis.x) * 180 / 3.14;
				real_angle = atan2((max_point_AABB.z - min_point_AABB.z), (max_point_AABB.x - min_point_AABB.x)) * 180 / 3.14;
				if (z_angle>0) z_angle -= 90;
				else  z_angle += 90;
				if (z_angle < 0) real_angle = (-real_angle);
				lenght = sqrt((max_point_AABB.z - min_point_AABB.z)*(max_point_AABB.z - min_point_AABB.z) + (max_point_AABB.x - min_point_AABB.x)*(max_point_AABB.x - min_point_AABB.x));

				if (lenght > 0.15 && lenght < 0.27)
				{
					cout << "     z_angle,lenght:" << real_angle <<"  "<<lenght<< endl;
					all_cloud_cluster->operator+=(*cloud_cluster);
					pre_data[times][0] = ((max_point_AABB.x + min_point_AABB.x) / 2) * 1000;
					pre_data[times][1] = min_point_AABB.z * 1000;
					pre_data[times][2] = real_angle * 100;
					//cout << "all(size,x,weight,z,angle):" << cloud_cluster->size() << "   " << pre_data[times][1] << "  " << max_point_AABB.z - min_point_AABB.z << "  " << pre_data[times][2] << "  " << pre_data[times][3] << "  " << atan2((max_point_AABB.z - min_point_AABB.z), (max_point_AABB.x - min_point_AABB.x)) << endl;
					times++;
				}
			}

			if (times == 1)
			{
				x = -pre_data[0][0];
				z = pre_data[0][1];
				angle = -pre_data[0][2];
				send_data[0] = 2;
			}
			else if (times>1)
			{
				send_data[0] = 2;
				short temp_x, max_n;
				max_n = 0;
				temp_x = 30000;
				for (int i = 0; i < times; i++)
				{
					if (abs(pre_data[i][0]) <temp_x)
					{
						x = -pre_data[i][0]-3;
						z = pre_data[i][1];
						angle = -pre_data[i][2];

						temp_x = abs(pre_data[i][0]);
					}
				}
			}
			else
			{
				x = z = angle = 0;
				send_data[0] = 0;
			}

			std::cout << x / 10 << "  " << z / 10;
			send_data[1] = x;
			send_data[2] = (x >> 8);
			send_data[3] = z;
			send_data[4] = (z >> 8);
			send_data[5] = angle;
			send_data[6] = (angle >> 8);
			send_data[7] = 0x5A;
			myserialport.writeToCom_char((char*)&send_data, sizeof(send_data));
			std::cerr << "  end  Done: " << int(tt.toc()) << " ms\n\n";
			//system("pause");
			if (all_cloud_cluster->size()>0)
			viewer.showCloud(cloud_viewer);
			tt.tic();

	}
}

void SimpleOpenNIViewer::run()
{
	OpenNI::initialize();
	Array<DeviceInfo> aDeviceList;

	while(true)
	{
		OpenNI::enumerateDevices(&aDeviceList);
		while(aDeviceList.getSize()==0)
		{
			OpenNI::enumerateDevices(&aDeviceList);usleep(1);
		}
		// create a new grabber for OpenNI devices
		pcl::Grabber* interface = new pcl::io::OpenNI2Grabber();

		// make callback function from member function
		boost::function<void(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&)> f =
			boost::bind(&SimpleOpenNIViewer::cloud_cb_, this, _1);

		// connect callback function for desired signal
		boost::signals2::connection c = interface->registerCallback(f);

		// start receiving point clouds
		interface->start();

		while (!viewer.wasStopped())
		{
			OpenNI::enumerateDevices(&aDeviceList);
			if(aDeviceList.getSize()==0||mode!=0x02)break;
			boost::this_thread::sleep(boost::posix_time::seconds(0.001));
		}
		/*
		while (!viewer.wasStopped()&&flag!=1)
		{
		boost::this_thread::sleep(boost::posix_time::seconds(1));
		}
		*/
		// Stop the send_data acquisition
		interface->stop();
		delete interface;
		cout<<"getammo break"<<endl;
		if(mode!=0x02)break;
	}
}

